Outline

    A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...
    14th
    World Congress ofTFAC
    Copyright
    to
    1999 IFAC
    B-ld-lO.,S
    14th Triennial World Congress, Beijing, P.R.
    China
    A
    FASTER
    ALGORITHM
    FOR
    CALCULATING
    mE
    INVERSE
    KINEMATICS
    OF
    A
    GENERAL
    6R
    MANIPULATOR
    FOR
    ROBOT
    REAL
    TIME
    CONTROL.
    Sebastian
    R.
    G6mez
    *,
    Jose
    A.
    Cerrada
    *,
    *
    DIEEC
    ETSII
    UNED
    Ciudad
    Universitaria
    sn
    28040
    Madrid, Spain.
    (sgomez,
    jcerrada
    @ ieec.uned.es)
    **
    ETS
    INDUSTRIALES
    Campus
    Universitario sn 13071 Ciudad Real, Spain
    (vfeliu@ind-cr.uclm.es)
    Abstract:
    This
    article
    presents
    a
    new
    faster
    algorithm
    for
    the
    real
    time
    calculation
    of
    the
    inverse kinematics
    of
    general,
    6R,
    six
    revolute
    jointed,
    manipulators.
    It
    solves
    the
    inverse
    kinematics
    problem
    towards a
    set
    of
    non-
    linear
    algebraic
    equations
    that
    are
    linearized
    obtaining
    a 16
    u
    ,
    degree
    polynomial
    in the
    tangent
    of
    the half-angle
    of
    a revolute
    joint.
    Elimination
    methods
    were
    originally presented by
    Lee
    and
    Liang.
    The
    same
    method
    was also
    derived
    by
    Raghavan
    and
    Roth,
    although
    the first solution with
    real
    time
    performance
    was presented
    by
    Manocha
    and
    Canny.
    The
    method
    proposed
    here
    is faster
    than
    the
    one
    from
    the last authors and also
    avoids
    the
    singularities
    that
    could
    appear
    in theirs.
    The
    aim
    of
    this algorithm
    is
    to
    find
    one
    of
    the 16 possible solutions
    in
    order
    to control a general
    6R
    manipulator
    in real time
    and
    perform
    path
    following.
    Using
    Rhaghavan
    and
    Roth
    method,
    the
    coefficients
    of
    the
    16-degree
    polynomial
    are
    calculated
    in real
    time
    by
    evaluating
    the symbolic
    determinant
    that
    generates
    this
    polynomial
    in 17 different points.
    The
    final solution is
    reached
    by
    a
    root
    solver,
    beginning
    the
    search
    in
    the
    last
    instant
    root.
    The
    average
    performance
    time
    of
    the
    algorithm
    is 25 ms in a
    PC
    Pentium
    at
    66
    MHz.
    This
    allows
    real
    time
    generation
    of
    the
    references
    for
    the
    local controllers
    of
    the actuators
    of
    the
    six
    joints
    that
    can
    be
    included
    in
    industrial manipulators. Copyright ©
    1999IFAC
    Keywords: Inverse Kinematics,
    Efficient
    Algorithms,
    Real
    Time
    Control,
    Robot
    Control,
    Robot
    Modeling.
    Copyright
    ©19991FAC
    1.
    INTRODUCTION
    The
    inverse
    kinematics
    problem
    is
    fundamental
    in
    the
    design
    and
    control
    of
    robot
    manipulators.
    The
    trajectories
    of
    robots
    are
    usually
    specified
    in terms
    of
    the
    end
    -
    effect
    pose,
    position
    and orientation, in
    Cartesian
    space, while its
    motion
    control
    is specified
    in
    terms
    of
    joint
    angles
    in
    joint
    space.
    The
    inverse
    kinematics
    gives
    the
    required
    joint
    angles
    to
    reach
    one
    desired
    pose
    with the
    end
    - effector.
    If
    a real
    time
    control
    is required, in real
    time
    path
    following,
    for
    instance,
    the
    inverse
    kinematics
    must
    generate
    joint
    angle references within
    the
    control
    period,
    which
    implies
    a
    very
    low
    time
    execution
    for the
    inverse kinematics algorithm.
    Such
    control
    period
    is
    of
    the
    order
    of
    0.01 seconds.
    Then
    a critical
    issue
    in
    robot
    control
    is
    to
    design
    algorithms
    to
    carry
    out
    the
    inverse
    kinematics·
    in
    a
    time
    of
    that
    order
    of
    magnitude.
    The
    more
    joints
    the
    robot
    has,
    the bigger
    is the
    complexity
    of
    the inverse kinematics.
    The
    most
    interesting case
    has
    been
    that
    of
    serial
    manipulators
    with six
    degrees
    of
    freedom,
    (DOF).
    Copyright 1999 IFAC
    With 6
    DOF,
    any
    pose
    in
    the
    space
    can
    be
    reached.
    The
    complexity
    of
    inverse
    kinematics
    in
    a general 6
    DOF
    manipulator
    is a function
    of
    its
    geometry.
    This
    complexity
    can
    be
    avoided
    by
    'adopting
    a
    "closed
    form"
    geometry
    that
    is
    when
    three
    consecutive
    axes
    intersect in a
    common
    point
    or
    they are parallel.
    The
    problems
    arise
    in
    the general case,
    when
    no
    such
    restrictions
    are
    applied
    to
    the
    manipulator.
    In
    fact
    most
    of
    6-DOF
    industrial manipulators
    use
    "closed
    form"
    configurations
    to
    avoid
    calculating the inverse
    kinematics
    of
    a general geometry.
    There
    are
    robot
    applications
    where
    the general
    robot
    geometry
    is
    more
    efficient
    than
    the
    before
    "closed
    forms".
    For
    example,
    when
    slave teleoperators
    are
    designed.
    The
    inverse
    kinematics
    problem
    of
    general
    configurations has
    been
    studied
    during
    the
    last three
    decades, particularly
    for
    6
    DOF
    manipulators.
    The
    first
    work
    was
    that
    of
    Pieper
    (1968),
    where
    he
    presented
    the
    first
    inverse
    kinematics
    solution
    based
    on
    iterative
    numerical
    techniques.
    He
    also
    presented
    the closed
    form
    solution.t'or a special geometry.
    ISBN:
    008
    0432484
    833
    A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...
    Roth
    et al.
    (1973)
    showed
    that
    this
    problem
    has
    an
    upper
    bound
    of
    32 solutions,
    that
    is,
    any
    6
    DOF
    manipulator
    could
    reach
    a goal position with his end
    - effector in 32 distinct configurations.
    The
    first
    constructive
    solution
    was
    given
    by
    Albala
    and
    Angeles
    (1979)
    in
    the
    form
    of
    a
    determinant
    of
    12x12
    matrix
    whose
    entries were quartic
    polynomials
    in
    the
    tangent
    of
    the
    half-angle
    of
    one
    of
    the
    joint
    variables.
    The
    next
    step
    was
    given
    by
    Duffy
    and
    Crane
    (1980)
    who
    provided
    a
    32-degree
    polynomial
    in the
    tangent
    of
    the
    half-angle
    using
    dyalitic elimination.
    This
    polynomial
    always
    presented
    extraneous
    or
    false
    solutions
    to
    the
    desired
    values.
    Tsai
    and
    Morgan,
    (1985)
    brought
    down
    the
    number
    of
    solutions
    to
    16.
    They
    cast
    the
    problem
    in a
    set
    of
    eight
    second-degree
    equations.
    The
    first
    conclusive
    proof
    of
    the
    existence
    of
    16
    solutions was
    given
    by
    Lee
    and
    Liang
    (1988),
    who
    derived
    a 16-
    degree
    polynomial
    in
    the
    tangent
    of
    the
    half- angle.
    Raghavan
    and Roth,
    (1990),
    derived
    this
    16-degree
    polynomial
    using
    dyalitic elimination.
    Manocha
    and
    Canny,
    (1992)
    were
    the
    first that
    presented
    a real
    time
    performance
    algorithm.
    They
    used the
    Raghavan
    and
    Roth's
    formulation
    solving
    the
    16-
    degree
    polynomial
    by
    transforming
    this
    in
    an
    eigenvalue
    problem.
    Their
    me
    thud
    was
    adopted
    in
    urder
    to
    avoid
    the
    expansion
    of
    a
    symbolic
    determinant,
    and
    the
    "ill
    conditioning"
    problems
    in
    solving
    the
    roots
    of
    a 16-
    degree polynomial.
    Although
    this
    method
    could
    perform
    in real
    time
    and
    avoid
    finding the
    polynomial
    roots, it
    generated
    new
    singularities that
    could
    appear
    and
    increase
    the
    time
    performance
    notoriously.
    Recently
    Castellet
    and
    Thomas,
    (1997) have
    presented
    a
    new
    approach
    using
    interval methods, but
    this solution
    doesn't
    perform
    in
    real
    time
    yet.
    This
    article
    describes
    a
    new
    real
    time
    performance
    algorithm
    to calculate
    the
    inverse
    kinematics
    of
    a 6R
    serial manipulator, which
    is
    more
    efficient
    than
    the
    present
    ones.
    It
    uses
    the
    Raghavan
    and
    Roth's
    formulation
    and
    determines
    the
    16-degree
    polynomial
    by
    evaluating
    the
    determinant
    that
    generates
    it
    in 17
    different
    points
    around
    the x origin.
    Under
    the basis
    of
    doing
    real
    time
    cuntrol
    for path
    following,
    only
    one
    of
    the
    16
    possible
    manipulator
    configurations
    is required.
    The
    angles
    at
    two
    consecutive
    instants will
    be
    close
    because
    the
    manipulator
    trajectories
    don't
    suffer
    from
    rough
    variations.
    This
    physical fact is used to select only
    one
    of
    the
    16
    polynomial
    roots, particularly
    the
    closest
    to the solution at
    the
    previous
    sampling
    instant. This
    new
    approach
    allows
    reducing
    dramatically
    the
    computation
    time
    of
    the
    inverse
    kinematics
    of
    robots
    with
    general
    configurations.
    It
    doesn't
    suffer
    in its
    efficiency
    from
    symbolic
    expansion
    because
    that
    is
    done
    before
    the algorithm
    starts
    to
    work.
    Neither
    from
    "ill
    conditioning"
    in
    solving
    the
    roots
    of
    the
    polynomial,
    as
    it
    is
    shown
    in
    the
    experimental
    results.
    It
    avoids
    the
    singularities
    problem
    because
    it
    doesn't
    use
    any
    matrix
    inversion
    in
    its process.
    This
    new
    method
    allows
    changing
    manipulator
    geometry
    in
    real time.
    For
    more
    of
    the
    examples
    it is
    Copyright 1999 IFAC
    14th World Congress ofTFAC
    able to
    calculate
    solutions with
    double
    precision
    in
    25
    ms
    in
    a
    PC
    Pentium at
    66
    MHz.
    That
    represents
    a
    25%
    of
    the
    performance
    time
    of
    Manocha's
    one.
    This
    algorithm
    was
    initially
    designed
    in
    (G6mez,1997)
    ,
    where
    more
    experimental
    results
    are
    shown.
    2.
    BACKGROUND
    In
    order
    to
    model
    a
    6R
    manipulator
    it is used the
    Denavit-Hartenberg,
    (1955)
    formalism.
    The
    transformation
    matrix
    that
    relates
    the
    position
    of
    the
    joint
    i+l
    to
    joint
    i is
    Ai
    -Si}·'
    sp
    ac
    1
    Ci).,
    C;J1:
    a;s;
    Pi
    ).i
    d;
    o 0 1
    (1)
    where
    8
    i
    represents
    the
    angle
    at
    the
    joint
    th,
    O:i
    is
    the
    twist
    angle
    between
    the
    axes
    j,h
    and i+
    1,
    Si
    =
    sin(
    8,),
    Ci
    ::;;
    cos(fJ,),
    Pi::;;
    sin(~),
    A;=cos(o:;),
    aiis
    the
    length
    of
    link
    i+
    1
    and
    d
    j
    is
    the
    offset
    distance
    at
    joint
    i.
    For
    a
    given
    6R
    manipulator
    we
    have
    ai'
    d
    j
    •
    J1;
    and Aj, and
    the
    pose
    of
    the
    end
    -
    effector
    given
    by
    A/land.
    [
    Ix
    mx
    nx
    qx
    1
    A = ly
    my
    ny
    qy
    hand
    l
    z m
    z
    n
    z
    qz
    o 0 0 1
    (2)
    The
    problem
    of
    inverse
    kinematics
    in
    a
    6R
    manipulator
    corresponds
    to
    computing
    the
    6
    angles
    e
    i
    such
    that
    A]A2A3A4A5A6
    ::;;
    A
    hand
    (3)
    What
    equation
    (3)
    represents
    are
    two
    4x4
    matrices.
    The
    entries
    of
    the
    left
    hand
    side are functions
    of
    the
    sines
    and
    cosines
    of
    8,.
    They
    also
    depends
    on
    manipulator
    geometry
    while
    the
    right
    hand
    side
    matrix
    depends
    on
    the
    end
    -
    effector
    pose.
    Both
    sides
    could
    change.
    Every
    time
    the
    end
    - effector
    moves,
    it
    does
    the
    A""nd
    matrix.
    So
    does
    the left
    matrix
    if
    manipulator
    geometry
    or
    joint
    angles
    are
    changed.
    Equation
    (3)
    corresponds
    to
    12
    scalar
    equations.
    Since
    the
    first
    3
    rows
    and 3 columns
    of
    matrix
    A, d
    are
    orthonormal,
    only
    6
    of
    the
    12
    equations
    ;'e
    independent.
    So
    the
    inverse
    kinematics
    problem
    of
    a
    6R
    manipulator
    consists
    on
    solving a
    system
    of
    6
    equations
    for
    6 unknowns. Unfortunately these 6
    equations
    are
    not
    linear
    in
    the
    unknowns
    and
    besides
    that;
    the
    unknowns
    are inside trigonometric
    functions.
    Three
    basic
    approaches
    have
    been
    done in
    order
    to
    solve
    this
    system:
    continuation
    methods,
    elimination
    methods
    and
    interval methods.
    The
    first
    is
    based
    in
    homotopy
    techniques
    to
    solve a
    system
    of
    polynomial equations. It
    computes
    the
    solutions
    following
    paths
    in
    the
    complex
    space.
    It
    is
    robust
    but
    slow
    for real time purposes.
    The
    third is
    based
    in interval
    methods
    for solving
    systems
    of
    non
    linear equations, it is also robust
    but
    slow
    too.
    The
    second
    tendency,
    which
    solves
    the
    system
    towards
    a
    ISBN:
    008
    0432484
    834
    A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...
    successive
    elimination
    of
    unknowns,
    is
    the
    basis
    of
    the
    method
    presented
    in this article.
    It
    wa~
    first
    developed
    by
    Raghavan
    and
    Roth,
    (1990)
    and
    used
    by
    Manocha
    and
    Canny
    (1992)
    in the fIrst
    real
    time
    algorithm.
    Both
    methods
    arc
    revised
    below.
    3.
    RAG
    HA
    V
    AN
    AND
    ROTH
    METHOD
    This
    section
    describes
    the
    elimination
    method
    proposed
    by
    these
    authors.
    It
    consists
    on
    reducing
    the
    non
    linear
    system
    to a
    16
    degree
    polynomial
    in
    tan(8Y2).
    Then
    solving
    the
    roots
    of
    this
    polynomial
    and
    finally
    computing
    the
    other
    angles
    by
    substitution
    in the
    elimination
    steps.
    Raghavan
    and
    Roth
    rearrange
    the
    equation
    (3)
    as
    A3A4AS =A;IA.l]A"andA6]
    (4)
    Then,
    they
    choose
    the 3
    rd
    and
    4th
    columns
    in
    both
    sides
    to
    obtain 6
    of
    the
    12
    equations
    which
    are
    free
    of
    8
    6
    •
    In
    fact, these 6
    equations
    represent
    the orientation
    and
    the
    position
    of
    the
    6
    th
    reference
    system
    expressed
    towards
    3rd,
    4t~
    and 5
    th
    references
    in
    the
    left
    hand
    side
    of
    (4)
    and
    expressed
    by
    the
    6
    th
    , 1
    st
    and 2
    nd
    references
    in the
    right
    hand
    side
    of
    (4),
    what
    they
    named
    as p
    (PI,
    P2,
    P3), and 1
    (1/,
    1
    2
    ,
    l,),
    vectors.
    See
    (Lee
    and
    Liang,1988).
    After
    this first
    elimination,
    they
    have
    a
    non-linear
    system
    of
    6
    equations
    and
    5
    unknowns.
    To
    obtain
    a linear
    system
    they
    took
    as
    new
    unknowns
    the
    products
    of
    sines
    and
    cosines
    in
    each
    side
    of
    (4)
    having
    54
    5
    5
    S4
    C
    5
    5]52
    C4
    5
    S
    5]C2
    C)5Z
    C4
    C
    S
    (5)
    A
    =B
    c]cZ
    54
    C
    4
    51
    Cl
    S5
    52
    Cs
    1
    Cz
    where
    A is a
    6x9
    matrix,
    whose
    entries
    are functions
    of
    53
    and C3 and B is
    6x8.
    Now
    the
    system
    (5)
    is
    linear
    but
    there are
    only
    6
    equations
    for
    16
    unknowns.
    As (5) is
    generated
    towards
    vectors p and
    I they
    obtain
    new
    equations
    by
    different
    products
    between
    them.
    Particularly they
    get
    8
    new
    equations
    from: p.
    p,p.
    l,px
    1,
    (p.
    p)l-
    (2p.l)p.
    With
    these
    new
    equations
    and
    (5)
    they obtain
    P
    (S455'
    s4c5,c4s5
    'C4CS'
    S4,
    C
    4
    ,
    5S
    ,csif
    =
    (6)
    Q
    (5I
    S
    2,5I
    C
    2,C]S2,C
    1
    C2,SI,C
    I
    ,S2,C2Y
    where
    P is a
    14x9
    matrix
    whose
    entries
    are functions
    of
    S3 and C3 and Q is
    14x8.
    The
    next step is
    the
    elimination
    of
    8
    1
    and
    (12
    in
    (6).
    That
    is
    done
    by
    pivoting
    in the
    terms
    of
    Q
    and
    translating
    the
    operations
    to the entries
    of
    P.
    Copyright 1999 IFAC
    14th World Congress ofTFAC
    That
    allows
    the
    elimination
    of
    (1/ and 8
    2
    in the last 6
    rows
    of
    (6),
    leading
    to
    the
    follow
    system
    (7)
    where
    I
    is
    a
    6x9
    matrix
    which
    entries
    are
    functions
    of
    53
    and
    C3'
    The
    next
    step
    is
    the
    substitution
    2x·
    I-x·
    Si
    =
    ___L')
    , C j ::::
    ~
    (8)
    l+xt
    l+Xj-
    where
    xi=tan(B,/2)
    Then
    (7)
    is
    transformed
    into
    l;'
    ( 2 2 2 2 2 2 T-0
    X4
    X
    S
    ,X4
    X
    S
    ,X
    4
    , X
    4
    Xs
    ,X
    4
    X
    S
    ,X
    4
    ,Xs
    ,xs,l
    -
    (9)
    where
    I'is
    a
    6x9
    matrix
    which
    entries are
    rational
    functions
    of
    X3'
    To
    clear
    the
    denominators
    of
    the
    4
    first
    rows,
    they
    are
    multiplied
    by
    (1 +
    x/).
    The
    system
    (9) is
    converted
    into a
    squared
    one
    using
    dialytic
    elimination,
    Raghavan
    and
    Roth
    7
    arraving
    to
    where
    l;"
    is
    a 12x12
    matrix
    ofthe
    form
    i'(
    4~,4'5
    ,4,~x;,~Xs
    ,4,X4X;,X4Xs
    ,X4'~'Xs,l
    J;;;0
    (la)
    As
    the
    system
    (10)
    should
    have
    a solution
    different
    from
    the
    trivial,
    it
    should
    be
    ..
    (L
    '
    L =
    o
    (11 )
    det
    (l:
    ) = 0
    (12)
    If
    this
    determinant
    is calculated, its
    expansion
    will
    give a
    16-degree
    polynomial
    in
    X3'
    Using
    a
    root
    solver,
    the
    values
    of
    XJ
    an;;
    calculated
    and afterwards,
    the
    value
    of
    (13.
    Substituting
    this
    value
    in
    (IO)
    they
    get
    8
    4
    and
    8
    5
    ,
    Then
    find 8
    1
    and 9
    2
    in (6)
    and
    8
    6
    in
    (3).
    Obviously,
    this is
    not
    a
    real
    time
    performance
    method,
    because
    the
    symbolic
    expansion
    of
    determinant
    IN
    couldn't
    be
    in
    real
    time.
    However
    it
    represents
    the
    foundations
    in
    the
    Manocha's
    method
    and
    also
    in
    the
    algorithm
    that
    is
    presented
    in
    this
    article.
    4.
    MANOCHA
    METHOD
    This
    method
    has
    been
    developed
    in
    (Manocha
    and
    Canny,
    1992),
    and
    (Manocha
    and
    Zhu,
    1994)
    and
    was
    the
    first
    real
    time
    inverse
    kinematics
    algorithm
    for
    generalized
    6R
    manipulators.
    To
    avoid
    the
    expansion
    of
    the
    determinant
    of
    IH
    in
    real
    time
    they used the
    Rhagavan
    and
    Roth
    approach
    but
    in a
    different
    way.
    They
    treat
    the
    ai,
    d;.
    f.1i
    and
    Ai
    and
    the
    A"and
    entries
    as
    symbolic
    constants
    and
    preprocess
    the
    entries
    of
    matrices
    P
    and
    Q in
    (6)
    as
    functions
    of
    this
    constants,
    being
    P
    and
    Q as
    P =P(a;, d
    i
    , f.Li' Ai' (13) i =3,4,5
    Q =
    Q(ai,d"pi'
    A"
    A/lam!) i = 1,2,6
    (13)
    ISBN:
    008
    0432484
    835
    A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...
    This
    maintains
    the
    algorithm
    adequate
    to
    any
    kind
    of
    geometry.
    Given
    a
    particular
    6R
    manipulator,
    they
    substitute
    the
    numerical
    values
    of
    ai,
    dj,
    J.li
    and
    Ai
    and
    the
    A'uwd
    in
    P
    and
    Q.
    The
    next step
    is
    the
    elimination
    of
    e
    1
    and e
    2
    ,
    for
    that
    purpose,
    thcy factorized P
    as
    (14)
    Then
    performing
    Gaussian
    elimination,
    pivoting
    on
    the
    entries
    of
    Q
    matrix
    and
    translating
    the
    operations
    to
    the entries
    of
    P
    l
    ,
    P
    2
    ,
    P3.
    they
    create
    matrix
    Ein
    (7)
    with the last 6
    rows
    of
    P
    and
    apply
    the trigometrical
    substitution
    of
    (8).
    This
    time
    they
    clean
    all
    the
    denominators
    of
    I"in
    (9)
    that
    allows
    a.E
    N
    of
    (10)
    that
    ean
    be
    factorized
    as
    LOO
    =
    Axi
    +
    BX3
    + C (15)
    Applying
    the
    restriction
    of
    (12)
    the
    problem
    leads
    to
    solve
    the
    following
    equation
    in
    Xj
    det(« +
    B~
    +
    C)
    =0
    (16)
    Manocha
    focused
    the
    solution
    through
    an
    eigenvalue
    problem.
    He
    established
    that
    the
    values
    of
    X3
    that satisfy
    (16)
    are
    the
    eigenvalues
    of
    the
    following
    matrix
    M
    M
    =(
    _AO-IC
    _:-I
    B
    )
    (17)
    where
    0
    and
    I are null
    and
    identity
    matrices
    respectively.
    This
    method
    also
    allows
    obtaining
    the
    values
    of
    X4
    and
    X5
    through
    the
    eigenvectors
    of
    M.
    With
    Xi',
    X4
    and
    X5.
    he
    gets
    eJ,
    (}4
    and
    (}5
    and
    the
    rest
    of
    the
    angles
    in the
    same
    way
    as
    Raghavan
    and
    Roth
    method.
    When
    matrix
    A
    becomes
    singular,
    Manocha
    proposes
    two
    alternatives:
    solve
    a
    generalized
    eigenvalue
    problem
    as
    M[
    -X3M
    2
    =0
    M[
    =[~
    ~J
    M2
    =(~
    ~)
    (18)
    or
    make
    certain
    transformations
    to
    the variable
    X3
    till
    the
    matrix
    A
    becomes
    non
    singular.
    In
    both
    cases
    the
    algorithm
    spends
    a
    lot
    of
    time,
    reducing
    its
    global
    efficiency.
    Manocha
    leads
    the
    solution
    to
    an
    eigenvalue
    problem
    to
    avoid
    expanding
    symbolic
    determinants
    and
    "ill
    conditioning"
    problems
    in
    finding
    roots
    of
    a 16-
    degree
    polynomial.
    But
    with
    his
    method
    some
    singularities
    could
    also
    appear.
    Besides
    that, this
    method
    always
    find the
    whole
    16
    solutions
    to
    the
    problem,
    It
    could
    seems
    positive,
    but
    if
    only
    one
    of
    the
    16 is
    required,
    for
    path
    following,
    there
    are a lot
    of
    resources
    wasted.
    In
    the
    next
    section a
    new
    method
    is
    proposed
    to find
    one
    of
    the
    16
    possible
    solutions
    avoiding
    singularities.
    5.
    ALGORITHM
    FORMULATION
    A
    new
    method
    for
    calculating
    the
    X3
    values
    in
    (16)
    is
    presented
    here.
    The
    main
    objective
    is
    to
    calculate
    one
    of
    the
    16
    possible
    values
    in
    order
    to
    control
    a
    6R
    manipulator.
    It
    is
    supposed
    that
    its
    movement
    starts
    at
    one
    pose,
    where
    the 6
    joint
    angles
    are
    known
    and
    it
    Copyright 1999 IFAC
    14th World Congress ofTFAC
    doesn't
    suffer
    great
    variations
    in
    its
    movement.
    Every
    x]
    calculated
    is
    done,
    by
    using
    the
    value
    of
    the
    previous
    instant.
    5.1Discussion
    The
    Manocha
    method
    avoids
    to
    calculate
    the
    polynomial
    roots
    with a
    root
    solver
    because
    the
    possible
    "ill
    conditioning"
    problems.
    III
    conditioned
    polynomials
    were
    first
    studied
    by
    Wilkinson,
    (1959),
    who
    established
    that
    solving
    the
    roots
    of
    polynomials
    of
    high
    degree,
    that
    is 19
    or
    higher,
    could
    be
    a
    "ill
    conditioned"
    problem. It
    could
    happen that a
    small
    perturbation
    in
    the
    coefficients
    of
    the
    polynomial
    brings
    great
    modifications
    in
    its
    roots.
    The
    real
    time
    inverse
    kinematics
    calculation
    of
    Manocha
    leads
    to
    find
    the
    solutions
    of
    equation
    (16),
    which
    is a
    24
    degree
    polynomiaL
    Trying
    to
    find
    the
    roots
    of
    this
    polynomial
    will
    require
    a very
    high
    precision
    root
    solver
    to
    avoid
    the"
    ill
    condition"
    of
    the
    polynomial,
    and
    that
    means
    time
    waste.
    This
    was
    the
    main
    reason
    why
    Manocha
    method
    avoids
    root
    solving.
    Equation
    (16)
    could
    be
    solved
    in
    a
    different
    way.
    avoiding
    "ill
    conditioning"
    problems
    and
    the
    singularities
    of
    the"
    eigenvalue
    way".
    If
    we
    name
    R(xj)
    to
    the
    polynomial
    generated
    by
    (16),
    that
    is
    R(x3)
    =
    det(Axi
    + BX3 +
    C)
    (19)
    We
    have
    that
    R (Xl) is
    24
    degree
    polynomial,
    but
    we
    also
    have
    that
    (1 +
    X/)
    4
    divides
    it.
    As
    it
    is
    shown
    in
    (Raghavan
    and
    Roth,
    1990)
    and
    (Manocha
    and
    Canny,1992).
    therefore we
    have
    a
    new
    16
    degree
    polynomial
    S(X3)
    such
    that
    R(.\3) =S(.\3)0+
    xi)4
    (20)
    And
    we
    have
    that
    2
    Sex
    ) =
    det(
    Ax
    3 +
    BX3
    +
    C)
    3
    (1
    +
    X;)4
    (2l)
    This
    new
    algorithm
    calculates
    the
    polynomial
    S (X3)
    by
    evaluating
    the
    expression
    (21)
    in
    17
    X3
    different
    points
    around
    X,=O.
    These
    points
    should
    be
    in
    an
    interval
    [-7..8] to
    avoid
    out
    of
    range
    values
    when
    evaluating
    higher
    powers
    of
    X,
    in
    S(X3J.
    By
    this
    method
    we
    get
    a square
    linear
    system
    of
    17
    equations,
    where
    the
    unknowns
    are
    the 17
    coefficients
    of
    the 16-degree
    polynomiaL
    Solving
    the
    system
    we
    have
    the
    polynomial.
    Now
    we
    have
    a 16-degree
    polynomial
    that
    doesn't
    suffer
    from
    "ill
    conditioned"
    problems
    and
    whose
    roots
    can
    be
    easily
    found.
    5.2
    Execution
    steps
    The
    algorithm
    follows
    the
    next
    execution
    steps:
    1.
    Following
    the
    Manocha
    method,
    we
    generate
    the
    entries
    of
    matrices
    P and Q
    of
    (6)
    as functions
    of
    the
    Denavit-Hartenberg
    parameters,
    ai,
    d
    i
    .
    /.1;
    and
    A,
    and the A
    hand
    •
    using
    the
    computer
    algebra
    systcm
    MAPLE.
    That
    is
    done
    once
    for
    each
    general
    6R
    manipulator.
    2.
    Given
    a particular
    manipulator
    geometry,
    substitute
    the
    numerical
    values
    of
    Denavit-
    Hartenberg
    parameters
    in P
    and
    Q entries.
    ISBN:
    008
    0432484
    836
    A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...
    3.
    Eliminate
    the
    variables
    tJ
    1
    and
    tJ
    2
    from
    (6)
    using
    Gaussian
    elimination.
    4.
    After
    this
    elimination,
    we
    obtain
    the
    matrices
    A,
    Band
    C
    of
    (15)
    5.
    Evaluate
    de
    expression
    (2l)
    in
    17
    different
    points
    to
    obtain
    the 17
    -coefficient
    S
    (xJ)
    6.
    Find
    one
    of
    the
    roots
    of
    S
    (x,)
    by
    a
    root
    solver,
    particularly
    the
    one
    closer
    to
    the
    last
    solution
    of
    the
    problem.
    6.
    IMPLEMENTATION
    AND
    PERFORMANCE
    The
    new
    algorithm
    and
    the
    Manocha
    method
    have
    been
    implemented
    to
    compare
    their
    time
    performance
    efficiency.
    Both
    algorithms
    have
    been
    created
    in C
    code,
    using
    the
    Watcom
    C
    ++
    compiler
    in
    order
    to
    obtain
    the fastest
    possible
    code.
    The
    functions
    obtained
    have
    been
    executed
    in a
    PC
    Pentium
    at
    66
    MHz.
    The
    gaussian
    elimination,
    the
    eigenvalue
    algorithm
    and
    the
    root
    solver
    comes
    from
    (Press
    et
    al
    1988).
    Given
    a
    particular
    manipulator
    geometry
    and
    a
    pose
    for
    its
    end-effector
    we
    have
    calculate
    the
    solution
    of
    the
    inverse
    kinematics
    problem
    by
    the
    two
    ways,
    Manocha
    and
    rootsolving.
    Besides
    that,
    we
    have
    implemented
    a trajectory
    for
    the
    end
    -
    effector
    by
    adding
    increments
    to
    its x position.
    In
    all the
    problems
    the
    algorithm
    takes
    about
    25
    ms
    on
    average
    on
    a
    PC
    Pentium
    at
    66MHz.
    That
    is the
    25%
    of
    Manocha
    method.
    In
    some
    configurations
    matrices
    P
    and
    Q
    could
    be
    range
    deficient,
    which
    leads
    to
    less
    than
    16
    solutions.
    In
    this
    cases
    the
    range
    of
    matrix
    a
    IHwill
    be
    evalualed,
    and
    depending
    on
    it,
    we
    will
    search
    for
    8,or
    4
    degree
    polynomial
    in
    X3
    using
    the
    same
    method.
    The
    rest
    of
    angles
    could
    be
    calculated
    using
    the
    method
    presented
    in
    (Manocha
    and
    Zhu,
    1994).
    6.1
    Examples
    The
    manipulator
    chosed
    was
    presented
    in
    (Raghavan
    and
    Roth,
    1990)
    an
    is
    determined
    by
    TABLEl
    TABLE
    /;
    Denavit-Hartenberg
    parameters
    ora
    6r
    manipulator
    ai
    d;
    ai
    0.8
    0.9
    20.0
    1.2 3.7 0.33
    0.33
    1.0
    45.0
    1.8
    0.5
    81.0
    0.6
    2.1
    12.0
    2.2
    0.63
    100.0
    The
    A
    hand
    is
    placed
    in the
    following
    pose
    [
    -.17967 -.9049 -.04723
    .2.5880]
    A - -.7683 .3857 .4609 -3.104
    hand
    - .4803 -.1796 -.8584 6.9163
    o 0 0 1
    Copyright 1999 IFAC
    14th World Congress ofTFAC
    The
    polynomial
    S(X3) is
    of
    the
    form
    S(X3)
    =1.0871 +
    2.o054x
    3
    +1.5436x; +7.110
    lx~
    -
    1.2420xj
    + 2.6547
    x~
    +3.1608x~
    -
    9.9781x~
    +15.834x~
    -2.5832x~
    +14.1'22x;0 + 9.8013x;1
    12
    13
    14
    15
    + 1.7745x3 +4.7533x3 - .21500X3
    +.51036x3
    + 1.0000x1
    6
    Assuming
    that
    in
    the
    previous
    control
    step
    the
    solution
    found
    was
    x)=
    -0.70
    in
    the
    present
    control
    step
    we
    search
    for
    the
    root
    closest
    to the
    before
    and
    we
    get
    X3=
    -.7271,
    this
    leads
    to
    the
    next
    set
    of
    angles
    01
    =
    13.109720,°
    2
    =
    50.992538,
    O}
    =
    -72.044112
    8
    4
    =-7.1962516,11
    5
    =37.852304,116
    =6.8315183
    If
    the
    24
    polynomial
    R(X3) is
    considered
    R(x
    3
    )
    =
    +.615ge-l
    + .2544e16x
    3
    -.9467
    e16x;
    +
    .l558e17x~
    -.1
    52ge17xj
    +
    .1014e17x~
    -.4870e16x~
    + .1770e16xI
    -5010e15x~
    +
    .1
    128e15xr
    -.204geI4x~o
    +
    .3036e13x11-3695e12x~2
    +
    .3708el1x~3
    ..
    3075elOx~4
    + .2104e9xj5
    -.l184e8x~6
    +
    .5438e6xr
    ·.2018e5x~g
    +S97.3xj9
    -13.24x;o
    +.6181xi
    1
    +.2116xi
    2
    +.2894e-lxi
    3
    +.5665e-lxj-4
    root-solvers
    will
    meet
    the
    "ill
    conditioning"
    problems
    and, unless
    40
    digits
    of
    precision
    are used, the roots
    found
    will
    be
    different
    from
    the real ones.
    Any
    of
    the
    16
    possible
    configurations
    can
    be
    followed.
    For
    instance,
    in
    the next
    table
    it
    can
    be
    seen the
    evolution
    of
    another
    configuration,
    for the
    geometry
    and
    pose
    presented
    in
    this
    example,
    while
    the
    qz
    of
    the
    end
    effector
    is
    changed.
    TABLE
    //:
    Execution
    of
    one
    trajectorie
    5.3-0.05*0
    -.414214.0
    29.6
    -44.971.0
    -63.0
    10.
    5.3-0.05*1
    -.3585
    15.0 25.1
    -39.469.2
    -70.3 16.
    5.3-0.05*2
    .3100
    16.1
    21.0
    -34.4
    67.5
    -76.4
    22.
    5.3-0.05*3 .2653 17.2 17.0
    -29.765.6
    -81.7
    27.
    5.3-0.05*4
    -.2226
    18.4 13.1 -25.1
    63.6
    -86.3 31.
    5.3-0.05*5
    -.180619.6
    9.1
    -20.461.6
    -90.6
    35.
    5.3-0.05*6
    -.1381
    20.9
    5.0
    -15.759.4
    -94.4
    39.
    5.3-0.05*7
    -.093622.4
    0.6
    -10.657.0
    -97.5 43.
    5.3-0.05*8
    -.044724.1
    4.3
    -5.1
    54.2
    -101.054.
    5.3-0.05*9
    .0140
    26.2
    10.5 1.6
    50.9
    -lO3.670.
    5.3-0.05*10.16677
    -1293i
    As
    it
    can
    he
    seen,
    when
    the
    end
    -
    effector
    reaches
    the
    position
    where
    q,
    =5.3-0.5,
    XJ
    gives a
    complex
    value
    with
    non-null
    imaginary
    part.
    This
    means
    that
    this
    position
    is
    not
    allowed
    for
    the
    end-
    effector
    with this
    geometric
    and
    pose
    constrains.
    For
    these
    examples
    the
    average
    running
    time
    of
    Manocha
    method
    is
    50
    ms
    while
    this
    new
    method
    wasted
    only
    25
    ms.
    In
    the
    follow
    example,
    it
    be
    can
    seen
    a
    contiguration
    that
    makes
    singular
    the
    matrix
    A in
    (16).
    ISBN:
    008
    0432484
    837
    A FASTER ALGORITHM FOR CALCULATING THE INVERSE KIN...
    This
    implies
    the
    generalized
    eigenvalue
    problem
    in
    Manocha
    method.
    The
    problem
    geometry
    is
    the
    same
    as
    in
    TABLE
    I.
    This
    time
    The
    Ahana
    is
    placed
    in
    the
    following
    pose
    A - -.7683 .3857 0.4609 -3.1046
    [
    -.1796-.9049 -.04723 -2.5880
    1
    }u.md
    -
    .4803-.1796
    -.8584 6.9163
    o 0 0 1
    Using
    the
    method
    presented
    here,
    we
    get
    the
    next
    sets
    of
    angles
    8
    1
    =13.109720,8
    2
    =50.992538,8
    3
    =-179.999999
    8
    4
    =-7.196251,8
    5
    =
    37.852304,8
    6
    =6.831518
    Now
    Manocha
    method
    wastes
    100
    ms
    while
    the
    new
    algorithm
    remains
    in
    25
    ms.
    7.
    CONCLUSIONS
    In
    this
    paper
    we
    present
    a
    new
    algorithm
    for
    inverse
    kinematics
    of
    a
    general6R
    manipulator,
    more
    efficient
    and
    robust
    that
    the
    present
    ones.
    The
    algorithm
    performs
    polynomial
    construction
    by
    evaluating
    a
    determinant
    in
    different
    points.
    Using
    a
    root
    solver,
    it finds
    the
    solution
    of
    the
    problem
    avoiding
    "ill
    conditioning",
    because
    of
    the
    low
    degree
    of
    the
    polynomial.
    The
    algorithm
    has
    been
    tested
    in a
    variety
    of
    instances.
    We
    have
    also
    tested
    in
    trajectories
    generation.
    In all
    cases
    the
    running
    time
    was
    of
    25
    ms
    in
    a
    PC
    Pentium
    at
    66
    MHz,
    which
    represents
    an
    improvement
    of
    25%
    over
    the
    presents.
    In
    those
    cases
    where
    others
    slow
    down,
    because
    degenerated
    configurations,
    8
    3
    =180.0
    this
    algorithm
    performs
    at
    the
    same
    rate.
    Execution
    times
    of
    order
    of
    cents
    of
    seconds
    make
    this
    algorithm
    more
    adequate
    than
    the
    present
    ones
    to
    the
    real
    time
    control
    of
    robots
    whose
    configurations
    are
    generalized,
    using
    low
    performance
    computer
    systems
    as
    a
    PC
    Pentium
    8.
    ACKNOWLEDGMENTS
    This
    work
    has
    been
    supported
    by
    the
    Spanish
    CICYT
    under
    grant
    TAP-96-1028-C02-02.
    9.
    REFERENCES
    Albala
    H.
    and
    Angeles
    1.
    (1979).
    Numerical
    solution
    to
    the
    input
    output
    displacement
    equation
    of
    the
    general
    7r
    spatial
    mechanism.
    In:
    Proceedings
    of
    the Fifth World Congress
    on
    Theory
    of
    Machines
    and
    Mechanisms.
    1008
    -101
    1.
    Castellet
    A.
    and
    Thomas
    F.
    (1997).
    Towards
    an
    Efficient
    Interval
    Method
    for
    Solving
    Inverse
    Kinematic
    Problems.
    In:
    Proceedings
    of
    IEEE
    Conference
    on
    Robotics
    and
    Automation. 3615-
    3620.
    Denavit
    J.
    and
    Hartenberg
    R.S.
    (1955).
    A
    kinematic
    notation
    for
    lower-pair
    mechanism
    based
    upon
    matrices.
    Joumal
    of
    applied
    Mechanics, 77:215-
    221.
    Duffy
    J.
    and
    Crane
    C.
    (1980).
    A
    displacement
    analysis
    of
    the general
    spatial
    7r
    mechanisms.
    In:
    Mechanism
    and
    Machine
    Theory
    15:153-169
    Copyright 1999 IFAC
    14th World Congress ofTFAC
    06mez
    S. R.
    (1997).
    Disefio
    de
    Algoritmos
    para
    la
    Representacion
    Cinematica
    de
    Solidos
    Articulados
    en
    Entomos
    Virtuales.
    Aplicaci6n
    a
    la
    Telepresencia.
    Ph.D.
    thesis,
    Universidad
    Nacional
    de
    Educaci6n
    a
    Distancia.
    Lee
    H.Y.
    and
    Liang
    C.O.
    (1988).
    A
    new
    vector
    Theory
    for
    the
    analysis
    of
    spatial
    mechanisms.
    In:
    Mechanism
    and
    Machine
    Theory, 23(3):209-
    217.
    Lee
    H.Y.
    and
    Liang
    C.G.
    (1988).
    Displacement
    analysis
    of
    the
    general
    spatial
    7-link
    7r
    mechanism.
    In:
    Mechanism
    and
    Machine
    Theory,
    23
    (3):219-226.
    Manocha
    D.
    And
    Canny
    J.F. (1992).
    Real
    time
    inverse
    Kinematics
    of
    general
    6R
    manipulators".
    In:
    Proceedings
    of
    IEEE
    Conference
    on
    Robotics
    and
    Automation. 383-389.
    Manocha
    D.
    and
    Zhu
    Y.(l994).
    A
    Fast
    Algorithm
    and
    System
    for
    the
    Inverse
    Kinematics
    of
    General
    Serial
    Manipulators_
    In:
    Proceedings
    Of
    IEEE
    Conference
    on
    Robotics
    and
    Automation.
    3348
    -
    3353.
    Pieper
    D. (1968).
    The
    Kinematics
    of
    Manipulators
    Under
    Computer
    Control.
    Ph.D.
    thesis,
    Stanford
    University
    Press
    W.,
    Flannery
    B.,
    Teukolsky
    S.
    and
    Vetterling
    W.
    (1988).
    Numerical
    Recipes
    in C.
    Cambrige.
    Raghavan
    M.
    and
    Roth
    B.
    (1990).
    A
    general
    solution
    for
    the
    inverse
    kinematics
    of
    all
    series
    chains.
    Proc.
    Of
    the
    8 Th
    CISM-IFTOMM
    Symposium
    on
    Robots
    and
    Manipu[ator.314 - 320,
    Roth
    B.,
    Rastegar
    J.
    and
    Scheinman
    V.
    (
    1973)
    On
    the
    Design
    of
    Computer
    Controled
    Manipulators.
    In: On Theory
    and
    Practice
    of
    Robots
    and
    Manipulators.
    93-113.
    First
    CISM
    IFToMM
    Symposium,
    Tsai
    L.W.
    and
    Morgan
    A.
    P.
    (1985).
    Solving
    the
    kinematics
    of
    the
    most
    general
    six
    and
    five-
    degree-of-freedom
    manipulators
    by
    continuation
    methods.
    In: Transactions
    of
    the
    ASME,
    Journal
    of
    Mechanisms,
    Transmissions
    and
    Automation
    in Design,
    107:
    189-200.
    Wilkinson
    J.H.
    (1959).
    The
    evaluation
    of
    the
    zeros
    of
    ill-conditioned
    polynomials.
    Parts
    i
    and
    ii.
    In:
    Numerical
    Mathematics. 150-166
    and
    167-180.
    ISBN:
    008
    0432484
    838